//RCU.pde
#include <WProgram.h>
#include "rfm70.h"
#include "Wire.h"
#include "WiiChuck.h"
//#include "Telemetry.h"

// comment out for no debug
//#define DEBUG_RCU
//#define DEBUG_DDU

//////////////////////////////////////////////////////////////////////
// Pin declarations
//////////////////////////////////////////////////////////////////////

#define PIN_LED 7
#define PIN_BTN 6
#define PIN_RFM_CE 8
#define PIN_RFM_IRQ 2

// RFM70 declarations
RFM70 rfm;


//////////////////////////////////////////////////////////////////////
// Protocol declarations
//////////////////////////////////////////////////////////////////////

#define TYPE_ACTOR              0
#define TYPE_SENSOR             1
#define TYPE_COMMANDER          2

#define MOT_STP 0x00
#define MOT_FWD 0x01
#define MOT_REV 0x02
#define MOT_BRK 0x03

#define ID_XDIVIDER             0x01 // rcu wiichuck actor
#define ID_XZEROZONE            0x02 // rcu wiichuck actor
#define ID_YZEROZONE            0x03 // rcu wiichuck actor
#define ID_PITCHZEROZONE        0x04 // rcu wiichuck actor
#define ID_ROLLZEROZONE         0x05 // rcu wiichuck actor
#define ID_PITCHTOLERANCE       0x06 // rcu wiichuck actor
#define ID_ROLLTOLERANCE        0x07 // rcu wiichuck actor
#define ID_CHUCKACTORENABLER    0x08 // rcu wiichuck actor
#define ID_CHUCKACTORDELAY      0x09 // rcu wiichuck actor
#define ID_CHUCKSENSORENABLER   0x0a // rcu wiichuck actor
#define ID_CHUCKSENSORDELAY     0x0b // rcu wiichuck actor
#define ID_JOYX                 0x0c // rcu wiichuck sensor
#define ID_JOYY                 0x0d // rcu wiichuck sensor
#define ID_PITCH                0x0e // rcu wiichuck sensor
#define ID_ROLL                 0x0f // rcu wiichuck sensor
#define ID_BTNC                 0x10 // rcu wiichuck sensor
#define ID_BTNZ                 0x11 // rcu wiichuck sensor
#define ID_INITIALIZER          0x12 // rcu and rob actor. val is the prog nr for rob
#define ID_STATUSREQUESTER      0x13 // rcu and rob actor. val is the requested id. 0 = broadcast
#define ID_TELEMETRYENABLER     0x14 // rob telemetry actor
#define ID_TELEMETRYDELAY       0x15 // rcu telemetry actor
#define ID_BATTERYSTATUS        0x16 // rob sensor
#define ID_SERVOPANTARGET       0x17 // rob telemetry actor
#define ID_SERVOTILTTARGET      0x18 // rob telemetry actor
#define ID_SERVODELAY           0x19 // rob telemetry actor
#define ID_SERVOPANPOSITION     0x1a // rob sensor
#define ID_SERVOTILTPOSITION    0x1b // rob sensor
#define ID_ENGINESENSEDELAY     0x1c // rob telemetry actor
#define ID_ENGINELPOWER         0x1d // rob telemetry actor
#define ID_ENGINELDIRECTION     0x1e // rob telemetry actor
#define ID_ENGINERPOWER         0x1f // rob telemetry actor
#define ID_ENGINERDIRECTION     0x20 // rob telemetry actor
#define ID_ENGINELSPEED         0x21 // rob sensor
#define ID_ENGINERSPEED         0x22 // rob sensor
#define ID_IRBARENABLER        0x23 // rob telemetry actor
#define ID_IR1DISTANCE          0x24 // rob sensor
#define ID_IR2DISTANCE          0x25 // rob sensor
#define ID_IR3DISTANCE          0x26 // rob sensor
#define ID_IR4DISTANCE          0x27 // rob sensor
#define ID_IR5DISTANCE          0x28 // rob sensor
#define ID_PINGDISTANCE         0x29 // rob sensor
#define ID_PINGENABLER       0x2a // rob telemetry actor
#define ID_SOUNDPLAY            0x2b // rob telemetry actor
#define ID_CAM_ENABLER          0x2c // rob telemetry actor
#define ID_MOCS_TURN           0x2d // rob telemetry actor
//#define ID_MOCS_TURNL           0x2e // rob telemetry actor
#define ID_MOCS_DRIVESEC          0x2f // rob telemetry actor
//#define ID_MOCS_REVSEC          0x30 // rob telemetry actor
#define ID_CAVS_ENABLER         0x31 // rob telemetry actor
#define ID_FAVS_ENABLER         0x32 // rob telemetry actor
#define ID_HEARTBEAT            0x33 // Heartbeat-Message to rob from rcu (or ddu)
#define ID_MOVEPANTILT          0x34 // rob telemetry actor
#define ID_BTNCZ                0x35 // rob telemetry actor
#define ID_TELEMETRY            0x36  // the telemetry sensor data from rob to rcu
#define ID_LIGHTS               0x37  // rob actor: lightsystem
#define ID_BTNS                 0x38  // rob sensor: buttonstate

//////////////////////////////////////////////////////////////////////
// General
//////////////////////////////////////////////////////////////////////

unsigned long actualMillis;


#ifdef DEBUG_RCU
void myDebug(uint8_t code) {
  switch (code) {
  case 0x01:
    Serial.println("received payload");
    break;
  case 0x02:
    Serial.println("sent payload");
    break;
  case 0x03:
    Serial.println("timeout");
    break;
  case 0x04:
    Serial.print("debug registers: ");
    for(int j=0; j<10; j++)
    {
      Serial.print(j,HEX);
      Serial.print(":");
      Serial.print(rfm.readRegVal(j), HEX);
      Serial.print(", ");
    }
    Serial.print(0x17,HEX);
    Serial.print(":");
    Serial.print(rfm.readRegVal(0x17), HEX);
    Serial.print(", carrier: ");
    Serial.print(rfm.getCarrierDetect(), HEX);
    Serial.println("");
    break;
  }
}
#endif



//////////////////////////////////////////////////////////////////////
// LED
//////////////////////////////////////////////////////////////////////

// Led declarations
byte rcvLed; // 1 if we received ping or sensor from rob. will be reset by parent to 0
int rcvLedDelay; // millis to be on
unsigned long rcvLedLastMillis;


void ledSetup() {
  //LED Setup
  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, HIGH);
  rcvLed = 1;
  rcvLedDelay = 100;
  rcvLedLastMillis = 0;
}


void ledHandler() {
  // control LED
  if(actualMillis - rcvLedLastMillis  > rcvLedDelay)
    digitalWrite(PIN_LED, LOW);
  if(rcvLed) {
    digitalWrite(PIN_LED, HIGH);
    rcvLed = 0;
    rcvLedLastMillis = actualMillis;
  }
}


//////////////////////////////////////////////////////////////////////
// Button
//////////////////////////////////////////////////////////////////////


void btnSetup() {
  // Btn Setup
  pinMode(PIN_BTN, INPUT);
}

void btnHandler() {
  // to be done
}




//////////////////////////////////////////////////////////////////////
// ROB: RFM70-Connection
//////////////////////////////////////////////////////////////////////


// RFM70 declarations
unsigned long upLastMillis; // last time we used uplink to rob
byte upDelay; // max delay * 10 Millis between two signals to rob
byte rcv_payload[RFM70_MAX_PACKET_LEN]; //32
byte old_rcv_payload[RFM70_MAX_PACKET_LEN]; //32
byte rcv_IDs[] = {
  ID_TELEMETRY, ID_ENGINELSPEED, ID_ENGINERSPEED, ID_SERVOPANPOSITION, ID_SERVOTILTPOSITION, ID_IR1DISTANCE, 
  ID_IR2DISTANCE, ID_IR3DISTANCE, ID_IR4DISTANCE, ID_IR5DISTANCE, ID_PINGDISTANCE, 
  ID_BATTERYSTATUS, ID_BTNS, 
  ID_TELEMETRYENABLER, ID_SERVODELAY, ID_ENGINESENSEDELAY, ID_ENGINELPOWER, ID_ENGINERPOWER, 
  ID_ENGINELDIRECTION, ID_IRBARENABLER, ID_PINGENABLER, ID_CAM_ENABLER, 
  ID_CAVS_ENABLER, ID_FAVS_ENABLER, ID_SERVOPANTARGET,
  ID_SERVOTILTTARGET,ID_BTNCZ, ID_LIGHTS, ID_TELEMETRYDELAY,
  ID_MOCS_TURN, ID_MOCS_DRIVESEC, ID_SOUNDPLAY};
byte snd_payload[RFM70_MAX_PACKET_LEN]; //32

void rfm70Setup() {
  // RFM70 setup
  rfm.Begin(PIN_RFM_CE, PIN_RFM_IRQ);
  rfm.confAddrWidth(3);
  rfm.setMode(MODE_PTX);
#ifdef DEBUG_RCU
  myDebug(0x04); // show registers
#endif
  upLastMillis = 0;
  upDelay = 30;
}

void receiveFromRobAndDispatch() {

  memcpy ( old_rcv_payload, rcv_payload, sizeof(rcv_payload));
  byte len = rfm.receivePayload(rcv_payload);
  if (len) {
    rcvLed = 1;
#ifdef DEBUG_RCU
    myDebug(0x01);
    Serial.print("payload: ");
    for (int k=0; k< len; k++)
      Serial.print(rcv_payload[k],HEX);
    Serial.println("");
#endif

    if(rcv_payload[0] == ID_TELEMETRY) {

      for(int i=1; i<18; i++) {
        if(rcv_payload[i] != old_rcv_payload[i]) {
          dduTransmit(rcv_IDs[i], rcv_payload[i]);         
        }
      }
      if(rcv_payload[18] != old_rcv_payload[18]) {
        dduTransmit(ID_ENGINELDIRECTION, rcv_payload[18] & 0x03);         
        dduTransmit(ID_ENGINERDIRECTION, rcv_payload[18] >> 2);         
      }
      for(int i=19; i<32; i++) {
        if(rcv_payload[i] != old_rcv_payload[i]) {
          dduTransmit(rcv_IDs[i], rcv_payload[i]);         
        }
      }
    }
  }
}


void robTransmit(uint8_t * cmd_buf, uint8_t len) {

  if (rfm.txTimeout()) {
    rfm.flushTxFIFO();
    rfm.cliTimeout();
  }
  rfm.sendPayload(cmd_buf, len, WITH_ACK);
#ifdef DEBUG_RCU
  myDebug(0x04);
  myDebug(0x02);
  Serial.print("payload: ");
  for (int i=0; i< len; i++){
    Serial.print(cmd_buf[i],HEX);
    Serial.print(":");
  }
  Serial.println("");
#endif
  upLastMillis = actualMillis;
}



//////////////////////////////////////////////////////////////////////
// wiiChuck
//////////////////////////////////////////////////////////////////////

// Wiichuck Definitions
#define MAXANGLE 90
#define MINANGLE -90

// Wiichuck declarations
WiiChuck chuck = WiiChuck();
int angleStart, currentAngle;
int tillerStart = 0;
double angle;
int wiichuckLastPRMove; // remember the last chuck pitch sector (-,0,+)
byte wiichuckPitchZerozone; // Pitch Zerozone size /2
byte wiichuckRollZerozone; // Roll  Zerozone size /2
byte wiichuckPitchTolerance; // Pitch Smoothener
byte wiichuckRollTolerance; // Pitch Smoothener
byte wiichuckLastBtnState; // c: 0x02 z 0x01
byte wiichuckJoyXZerozone; // JoyX zerozone size /2
byte wiichuckJoyYZerozone; // JoyY zerozone size / 2
byte wiichuckJoyXDivider; // JoyX motor breaker divider

// Stream declarations
byte dnWiichuck; // 1 when we send Wiichuck data from rcu to the ddu
byte dnWiichuckDelay; // Will send wiichuck data to ddu every dnWiichuckDelay * 10 millis
unsigned long dnWiichuckLastMillis;
byte upWiichuck; // 1 when wiichuck shall be used to control rob. 0: only ddu controls rob
byte upWiichuckDelay; // Will send wiichuck data to rob every upWiichuckDelay * 10 millis
unsigned long upWiichuckLastMillis;

// Engine declarations for joystick movement
byte robLastEngineL;
byte robLastEngineR;


// Wiichuck setup
void wiichuckSetup() {
  chuck.begin();
  /* This delay is required for a wired nunchuk
   otherwise the data will appear maxed out */
  delay(1);
  chuck.update();
  chuck.calibrateJoy();
  wiichuckLastPRMove = 0x44; // zerozone
  wiichuckPitchZerozone = 20;
  wiichuckRollZerozone = 20;
  wiichuckPitchTolerance = 10;
  wiichuckRollTolerance = 10;
  wiichuckLastBtnState = chuck.buttonC << 1 | chuck.buttonZ;
  wiichuckJoyXZerozone = 10;
  wiichuckJoyYZerozone = 10;
  wiichuckJoyXDivider = 100;

  // Stream setup
  dnWiichuck = 1; // 1 when we send Wiichuck data from rcu to the ddu
  dnWiichuckDelay = 10;
  upWiichuck = 1; // 1 whenn wiichuck shall be used to control rob. 0:   ddu controls rob
  upWiichuckDelay = 20;
  upWiichuckLastMillis = 0;
  dnWiichuckLastMillis = 0;

  // Rob Engine setup
  robLastEngineL = 0;
  robLastEngineR = 0;

}

uint8_t dduLastJoyX;
uint8_t dduLastJoyY;
uint8_t dduLastPitch;
uint8_t dduLastRoll;
uint8_t dduLastBtnC;
uint8_t dduLastBtnZ;

//wiiChuck handler
void wiichuckHandlerDDU() {
  // send Wiichuck to ddu

  if(dnWiichuck & actualMillis - dnWiichuckLastMillis  > dnWiichuckDelay * 10) {

    int joyX =  chuck.readJoyX();
    if (joyX <= 0 + wiichuckJoyXZerozone && joyX >= 0 - wiichuckJoyXZerozone) {
      if(dduLastJoyX != 0) {
        dduTransmit(ID_JOYX, 0);
        dduLastJoyX=0;
      }
    }
    else {
      if(dduLastJoyX != joyX) {
        dduTransmit(ID_JOYX, joyX);
        dduLastJoyX=joyX;
      }
    }

    int joyY =  chuck.readJoyY();
    if (joyY <= 0 + wiichuckJoyYZerozone && joyY >= 0 - wiichuckJoyYZerozone) {
      if(dduLastJoyY != 0) {
        dduTransmit(ID_JOYY, 0);
        dduLastJoyY=0;
      }
    }
    else {
      if(dduLastJoyY != joyY) {
        dduTransmit(ID_JOYY, joyY);
        dduLastJoyY=joyY;
      }
    }

    if(dduLastBtnC != chuck.buttonC) {
      dduTransmit(ID_BTNC, chuck.buttonC);
      dduLastBtnC=chuck.buttonC;
    }

    if(dduLastBtnZ != chuck.buttonZ) {
      dduTransmit(ID_BTNZ, chuck.buttonZ);
      dduLastBtnZ=chuck.buttonZ;
    }


    int cpitch = chuck.readPitch();
    if(cpitch >= 90 + wiichuckPitchTolerance + wiichuckPitchZerozone
      ||cpitch <= 90 - wiichuckPitchTolerance - wiichuckPitchZerozone ) {
      if(dduLastPitch != (byte) cpitch - 90) {
        dduTransmit(ID_PITCH, (byte) cpitch - 90);
        dduLastPitch =(byte) cpitch - 90;
      }
    }
    else {
      if(dduLastPitch != 0) {
        dduTransmit(ID_PITCH, 0);
        dduLastPitch=0;
      }
    }

    int croll = chuck.readRoll();
    if(croll >= 0 + wiichuckRollTolerance + wiichuckRollZerozone
      ||croll <= 0 - wiichuckRollTolerance - wiichuckRollZerozone ) {
      if(dduLastRoll != (byte) croll) {
        dduTransmit(ID_ROLL, (byte) croll);
        dduLastRoll =(byte) croll;
      }
    }
    else {
      if(dduLastRoll != 0) {
        dduTransmit(ID_ROLL, 0);
        dduLastRoll=0;
      }
    }    

    dnWiichuckLastMillis = actualMillis;
  }
}



void wiichuckHandlerRob() {
  // send Wiichuck to rob
  if((upWiichuck != 0)  && (actualMillis - upWiichuckLastMillis  >
    upWiichuckDelay * 10)) {

    byte payloadlen = 0;
    byte pos = payloadlen;

    //pantilt
    snd_payload[pos++] = ID_MOVEPANTILT;
    snd_payload[pos] = wiichuckLastPRMove; //reset

    int cpitch =  chuck.readPitch(); // dn = 0 up = 200
    int croll =  chuck.readRoll(); // left = -70 right = + 70
    if (cpitch >= 90 + wiichuckPitchTolerance + wiichuckPitchZerozone )
      snd_payload[pos] = (snd_payload[pos] & 0xF0) | 0x01;
    else if (cpitch <= 90 - wiichuckPitchTolerance - wiichuckPitchZerozone)
      snd_payload[pos] = (snd_payload[pos] & 0xF0) | 0x02;
    else if (cpitch < 90 + wiichuckPitchZerozone  && cpitch > 90 - wiichuckPitchZerozone)
      snd_payload[pos] = (snd_payload[pos] & 0xF0);
    if (croll >= 0 + wiichuckRollTolerance + wiichuckRollZerozone )
      snd_payload[pos] = (snd_payload[pos] & 0x0F) | 0x20;
    else if (croll <= 0 - wiichuckRollTolerance - wiichuckRollZerozone)
      snd_payload[pos] = (snd_payload[pos] & 0x0F) | 0x10;
    else if (croll < 0 + wiichuckRollZerozone  && croll > 0 - wiichuckRollZerozone)
      snd_payload[pos] = (snd_payload[pos] & 0x0F);
    if(snd_payload[pos] != wiichuckLastPRMove) {
      wiichuckLastPRMove = snd_payload[pos];
      payloadlen = 2;
    }

    //buttonstate
    pos = payloadlen;
    snd_payload[pos++] = ID_BTNCZ;
    snd_payload[pos] = chuck.buttonC << 1 |chuck.buttonZ;
    if (snd_payload[pos] != wiichuckLastBtnState) {
      wiichuckLastBtnState = snd_payload[pos];
      payloadlen +=2;
    }

    //Joystick
    byte robEngineL;
    byte robEngineR;
    pos = payloadlen;
    snd_payload[pos++] = ID_ENGINELDIRECTION;
    snd_payload[pos++] = MOT_FWD; // will be set later // pos -7
    snd_payload[pos++] = ID_ENGINERDIRECTION;
    snd_payload[pos++] = MOT_FWD; // will be set later // pos -5
    snd_payload[pos++] = ID_ENGINELPOWER;
    snd_payload[pos++] = 0; // will be set later // pos -3
    snd_payload[pos++] = ID_ENGINERPOWER;
    snd_payload[pos++] = 0; // will be set later // pos -1
    int joyX =  chuck.readJoyX();
    int joyY =  chuck.readJoyY();
    // zerozones
    if ((joyX > 0 + wiichuckJoyXZerozone || joyX < 0 -
      wiichuckJoyXZerozone) || (joyY > 0 + wiichuckJoyYZerozone || joyY < 0
      - wiichuckJoyYZerozone) ) {
      // scaler
      int engineLPwr = (int8_t)joyY; // robEngineLPower
      int engineRPwr = (int8_t)joyY; // robEngineLPower

      // breaker
      if (joyX > 0 + wiichuckJoyXZerozone) {
        engineRPwr -= (int8_t)((float)joyY /
        (float)wiichuckJoyXDivider * joyX);
      }
      if (joyX < 0 - wiichuckJoyXZerozone) {
        engineLPwr -= (int8_t)((float)joyY /
        (float)wiichuckJoyXDivider * -joyX);
      }
      if(engineRPwr < 0) {
        engineRPwr *= -1;
        snd_payload[pos - 5] = MOT_REV;
      }
      snd_payload[pos - 1] = engineRPwr;

      if(engineLPwr < 0) {
        engineLPwr *= -1;
        snd_payload[pos - 7] = MOT_REV;
      }
      snd_payload[pos - 3] = engineLPwr;

    }
    else {
      snd_payload[pos - 3]   = 0; // robEngineL
      snd_payload[pos - 7] = MOT_STP;
      snd_payload[pos - 1] = 0; // robEngineR
      snd_payload[pos - 5] = MOT_STP;
    }
    // sender
    if (snd_payload[pos - 3] != robLastEngineL || snd_payload[pos - 1] !=  robLastEngineR) {
      robLastEngineL = snd_payload[pos - 3];
      robLastEngineR = snd_payload[pos - 1];
      payloadlen += 8;
    }
    if(payloadlen) {
      robTransmit(snd_payload, payloadlen);
      upWiichuckLastMillis = actualMillis;
    }
  }
}



//////////////////////////////////////////////////////////////////////
// DDU
//////////////////////////////////////////////////////////////////////

// Serial declarations
//#define SERIAL_SPEED 115200
#define SERIAL_SPEED 38400

void dduSetup() {
  Serial.begin(SERIAL_SPEED);
}


uint16_t receiveFromDDU() {
  // check if command (16 bit) data is available and get it
  int incomingMSB = 0;
  int incomingLSB = 0;
  if (Serial.available() > 1) {

    // read the incoming byte (MSB)
    incomingMSB = Serial.read();
    // reflect
    //Serial.write((byte)incomingMSB);

    // read the incoming byte (LSB)
    incomingLSB = Serial.read();
    // reflect
    //Serial.write((byte)incomingLSB);

    return (incomingMSB << 8) | (incomingLSB & 0xFF);
  }
  return -1;
}


void dispatchDDUCmd(uint16_t theCmd) {
  if(theCmd == -1)
    return;

  uint8_t _cmd = theCmd >> 8;
  uint8_t _val = theCmd & 0xFF;

  switch (_cmd) {
    //
    // rcu wiichuck Actors
    //
  case ID_XDIVIDER:
    wiichuckJoyXDivider = _val;
    break;
  case ID_XZEROZONE:
    wiichuckJoyXZerozone = _val;
    break;
  case ID_YZEROZONE:
    wiichuckJoyYZerozone = _val;
    break;
  case ID_PITCHZEROZONE:
    wiichuckPitchZerozone = _val;
    break;
  case ID_ROLLZEROZONE:
    wiichuckRollZerozone = _val;
    break;
  case ID_PITCHTOLERANCE:
    wiichuckPitchTolerance = _val;
    break;
  case ID_ROLLTOLERANCE:
    wiichuckRollTolerance = _val;
    break;
  case ID_CHUCKACTORENABLER:
    upWiichuck = _val;
    break;
  case ID_CHUCKACTORDELAY:
    upWiichuckDelay = _val;
    break;
  case ID_CHUCKSENSORENABLER:
    dnWiichuck = _val;
    break;
  case ID_CHUCKSENSORDELAY:
    dnWiichuckDelay = _val;
    break;
    //
    // rcu other actors
    //
  case ID_HEARTBEAT:
    upDelay = _val;
    break;
  case ID_INITIALIZER:
    setup();
    snd_payload[0] = _cmd;
    snd_payload[1] = _val;
    robTransmit(snd_payload, 2);
    break;
  case ID_STATUSREQUESTER:
    dduTransmit(ID_HEARTBEAT, upDelay);
    dduTransmit(ID_CHUCKSENSORENABLER, dnWiichuck);
    dduTransmit(ID_CHUCKSENSORDELAY, dnWiichuckDelay);
    dduTransmit(ID_CHUCKACTORENABLER, upWiichuck);
    dduTransmit(ID_CHUCKACTORDELAY, upWiichuckDelay);
    dduTransmit(ID_PITCHZEROZONE, wiichuckPitchZerozone);
    dduTransmit(ID_ROLLZEROZONE, wiichuckRollZerozone);
    dduTransmit(ID_PITCHTOLERANCE, wiichuckPitchTolerance);
    dduTransmit(ID_ROLLTOLERANCE, wiichuckRollTolerance);
    dduTransmit(ID_XZEROZONE, wiichuckJoyXZerozone);
    dduTransmit(ID_YZEROZONE, wiichuckJoyYZerozone);
    dduTransmit(ID_XDIVIDER, wiichuckJoyXDivider);
    snd_payload[0] = _cmd;
    snd_payload[1] = _val;
    robTransmit(snd_payload, 2);
    break;
    //
    // rob actors
    //
  case ID_TELEMETRYENABLER:
  case ID_TELEMETRYDELAY:
  case ID_SERVOPANTARGET:
  case ID_SERVOTILTTARGET:
  case ID_SERVODELAY:
  case ID_ENGINESENSEDELAY:
  case ID_ENGINELPOWER:
  case ID_ENGINELDIRECTION:
  case ID_ENGINERPOWER:
  case ID_ENGINERDIRECTION:
  case ID_IRBARENABLER:
  case ID_PINGENABLER:
  case ID_SOUNDPLAY:
  case ID_CAM_ENABLER:
  case ID_MOCS_TURN:
  case ID_MOCS_DRIVESEC:
  case ID_CAVS_ENABLER:
  case ID_FAVS_ENABLER:
  case ID_LIGHTS:
    snd_payload[0] = _cmd;
    snd_payload[1] = _val;
    robTransmit(snd_payload, 2);
    break;
  default:
    break;
  }
}


void dduTransmit(uint8_t _cmd, uint8_t _val){
#ifdef DEBUG_DDU
  Serial.print("<");
  Serial.print(_cmd,HEX);
  Serial.print(":");
  Serial.print(_val,HEX);
  Serial.println(">");
#else
  Serial.write(_cmd);
  Serial.write(_val);
#endif
}



//////////////////////////////////////////////////////////////////////
// Setup
//////////////////////////////////////////////////////////////////////


void setup(void) {

  delay(3000);



  dduSetup();
  
  #ifdef DEBUG_DDU
  Serial.println("\n[rcu. rfm70 receiver. wiichuck. telemetry. serial=>ddu]");
#endif
  
  wiichuckSetup();
Serial.println("*");
  rfm70Setup();
Serial.println("*");
  ledSetup();
Serial.println("*");
  btnSetup();
Serial.println("*");  

}


//////////////////////////////////////////////////////////////////////
// Handlers
//////////////////////////////////////////////////////////////////////


void robHeartbeatHandler() {
  // send Heartbeat to rob because we didn send for at least upDelay * 10 Millis
  if(actualMillis - upLastMillis  > upDelay * 10) {
    snd_payload[0] = ID_HEARTBEAT;
    snd_payload[1] = 0x00;
    robTransmit(snd_payload, 2);
  }
}





//////////////////////////////////////////////////////////////////////
// Loop
//////////////////////////////////////////////////////////////////////


void loop(void) {
  actualMillis = millis();

  // wiichuck loop runner
  chuck.update();
  wiichuckHandlerDDU();
  wiichuckHandlerRob();

  // DDU receiver loop runner
  dispatchDDUCmd(receiveFromDDU());

  // Rob (RFM70) receive looprunner
  receiveFromRobAndDispatch();

  // Heartbeat loop Runner
  robHeartbeatHandler();

  // LED loopRunner
  ledHandler();

  // Button loopRunner
  btnHandler();


}


